# -*- coding: utf-8 -*-

###############################################################################
# Copyright [2010] [Roberto Carrasco - rcarrasco@talentus.cl]
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#         http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License. 
###############################################################################

"""Process Skytraq Binary Response

(1) Tips from Jesper Zedlitz, SkyTraq GPS data logger
"""

import math
import time
import datetime
import operator

import Utils
import Skytraq

class BinaryResponse:
    def __init__(self, _message):
        self.message = _message
        self.valid = False
        self.crc_ok = self.crc_check(self.message)
        if self.crc_ok:
            self.valid = True
            message = self.message
            self.messageId = message[4]
            if self.messageId == Skytraq.RSP_SOFTWARE_VERSION:
                self.softwareType = message[5]
                self.kernelVersion = '%s.%s.%s.%s'%(message[6], message[7], message[8], message[9])
                self.odmVersion = '%s.%s.%s.%s'%(message[10], message[11], message[12], message[13])
                self.revisionNumber = '%s.%s.%s.%s'%(message[14], message[15], message[16], message[17])
            elif self.messageId == Skytraq.RSP_SOFTWARE_CRC:
                self.softwareType = message[5]
                self.softwareCRC = Utils.toUINT16(message, 6)
            elif self.messageId == Skytraq.RSP_ACK:
                self.ackId = message[5]
            elif self.messageId == Skytraq.RSP_NACK :
                self.ackId = message[5]
            elif self.messageId == Skytraq.RSP_POSITION_UPDATE_RATE:
                self.updateRate = message[5]
            elif self.messageId == Skytraq.RSP_GPS_EPHEMERIS_DATA:
                self.sv_id = message[6] # Utils.toUINT16(message, 5)
                self.subframe_data = [[0 for i in range(28)] for i in range(3)]
                idx=7
                for i in range(3):
                    for j in range(28):
                        self.subframe_data[i][j] = message[idx]
                        idx = idx + 1
            elif self.messageId == Skytraq.RSP_GPS_DATUM:
                self.datum_index = Utils.toUINT16(message, 5)
            elif self.messageId == Skytraq.RSP_GPS_WAAS_STATUS:
                self.waas_status = message[5]
            elif self.messageId == Skytraq.RSP_GPS_POSITION_PINNING_STATUS:
                self.pinning_status = message[5]
            elif self.messageId == Skytraq.RSP_GPS_NAVIGATION_MODE:
                self.navigation_mode = message[5]
            elif self.messageId == Skytraq.RSP_GPS_ONEPPS_MODE:
                self.onepps_mode = message[5]
            elif self.messageId == Skytraq.RSP_LOG_STATUS_OUTPUT:
                self.currentLogBufferAddress = Utils.toUINT32(message, 5)
                self.sectorsLeft = Utils.toUINT16(message, 9)
                self.totalSectors = Utils.toUINT16(message, 11)
                self.maxTime = Utils.toUINT32(message, 13)
                self.minTime = Utils.toUINT32(message, 17)
                self.maxDistance = Utils.toUINT32(message, 21)
                self.minDistance = Utils.toUINT32(message, 25)
                self.maxSpeed = Utils.toUINT32(message, 29)
                self.minSpeed = Utils.toUINT32(message, 33)
                self.dataLogging = message[37]
                self.logFifoMode = message[38]
            else:
                self.valid=False

    def crc_check(self, bytes):
        # 0xa0 0xa1 HI LOW B1 B2 B3 B4 .... XX 0x0d 0x0a
        size=len(bytes)
        crc_received = bytes[size - 3]
        crc_calculated = reduce(operator.xor, bytes[4:size-3])
        return crc_calculated == crc_received

class SectorResponse:
    def __init__(self, _message):
        self.message = _message
        self.valid = False
        if len(_message) == 18:         # empty sector
            self.crc_ok=True
        else:
            self.crc_ok = self.crc_check(self.message)
        if self.crc_ok:
            self.valid = True
            self.sector = self.message[0:-18]

    def crc_check(self, bytes):
        # .... END 0x0 CHECKSUM=cs 0x00 0x00 0x0d 0x0a
        size=len(bytes)
        crc_received = bytes[size - 5]
        crc_calculated = reduce(operator.xor, bytes[:size-18])
        return crc_calculated == crc_received

def parseSectors(sectors):
    points = []
    minlat = 0
    maxlat = 0
    minlon = 0
    maxlon = 0
    l = len(sectors)
    i = 0
    pseq = 0
    while i < l:
        if sectors[i] == 0xE0:
            # EMPTY
            i = i + 2
            continue
        elif sectors[i] == 0x40:
            # 'FULL'
            V = Utils.toUINT16_BE(sectors, i + 0) & 0x03FF
            WN = (Utils.toUINT16_BE(sectors, i + 2) & 0x03FF) + 0x400   # See (1) for + 0x400
            TOW=((sectors[i + 2] >> 4) & 0x0F) + (Utils.toUINT16_BE(sectors, i + 4) << 4)
            X = Utils.toUINT16_BE(sectors, i + 6) + (Utils.toUINT16_BE(sectors, i + 8) << 16)
            Y = Utils.toUINT16_BE(sectors, i + 10) + (Utils.toUINT16_BE(sectors, i + 12) << 16)
            Z = Utils.toUINT16_BE(sectors, i + 14) + (Utils.toUINT16_BE(sectors, i + 16) << 16)
            if X & 0x80000000:
                X = int(-0x100000000 + X)
            if Y & 0x80000000:
                Y = int(-0x100000000 + Y)
            if Z & 0x80000000:
                Z = int(-0x100000000 + Z)
            T = 315964800 + 604800 * WN + TOW   # 315964800=6 January 1980 00:00:00 (epoch), 604800=60*60*24*7 (secs in a week)
            p = type('ANONYMOUS', (object,), dict())
            p.seq = pseq
            p.is_full = True
            p.is_compact = False
            p.is_upoi = False
            p.time = __timestamp2iso8601str(T)  #  See (1)
            p.speed = V
            (p.latitude, p.longitude, p.height) = __ecef2lla(X, Y, Z)
            points.append(p)
            i = i + 18
        elif sectors[i] == 0x80:
            # 'COMPACT'
            V = Utils.toUINT16_BE(sectors, i + 0) & 0x03FF
            DTOW = Utils.toUINT16_BE(sectors, i + 2)
            DX = (Utils.toUINT16_BE(sectors, i + 4) >>6) & 0x03FF
            DY =((sectors[i + 5] & 0x3F) | ((sectors[i + 6] >> 4) << 6)) & 0x03FF
            DZ = Utils.toUINT16_BE(sectors, i + 6) & 0x03FF
            if DX & 0x200:      #  See (1)
                DX = 0x1FF - DX
            if DY & 0x200:      #  See (1)
                DY = 0x1FF - DY
            if DZ & 0x200:      #  See (1)
                DZ = 0x1FF - DZ
            T = T + DTOW
            X = X + DX
            Y = Y + DY
            Z = Z + DZ
            p = type('ANONYMOUS', (object,), dict())
            p.seq = pseq
            p.is_full = False
            p.is_compact = True
            p.is_upoi = False
            p.time = __timestamp2iso8601str(T)    # See (1)
            p.speed = V
            (p.latitude, p.longitude, p.height) = __ecef2lla(X, Y, Z)
            points.append(p)
            i = i + 8
        elif sectors[i] == 0x60:
            # 'FULL UPOI'
            V = Utils.toUINT16_BE(sectors, i + 0) & 0x03FF
            WN = (Utils.toUINT16_BE(sectors, i + 2) & 0x03FF) + 0x400   # See (1) for + 0x400
            TOW=((sectors[i + 2] >> 4) & 0x0F) + (Utils.toUINT16_BE(sectors, i + 4) << 4)
            X = Utils.toUINT16_BE(sectors, i + 6) + (Utils.toUINT16_BE(sectors, i + 8) << 16)
            Y = Utils.toUINT16_BE(sectors, i + 10) + (Utils.toUINT16_BE(sectors, i + 12) << 16)
            Z = Utils.toUINT16_BE(sectors, i + 14) + (Utils.toUINT16_BE(sectors, i + 16) << 16)
            if X & 0x80000000:
                X = int(-0x100000000 + X)
            if Y & 0x80000000:
                Y = int(-0x100000000 + Y)
            if Z & 0x80000000:
                Z = int(-0x100000000 + Z)
            T = 315964800 + 604800 * WN + TOW   # 315964800=6 January 1980 00:00:00 (epoch), 604800=60*60*24*7 (secs in a week)
            p = type('ANONYMOUS', (object,), dict())
            p.seq = pseq
            p.is_full = False
            p.is_compact = False
            p.is_upoi = True
            p.time = __timestamp2iso8601str(T)  #  See (1)
            p.speed = V
            (p.latitude, p.longitude, p.height) = __ecef2lla(X, Y, Z)
            points.append(p)
            i = i + 18
        else:
            #print 'BAD HEADER'
            i = i + 1
            continue
        pseq = pseq + 1
        p.name = 'TP%05d' % pseq
        if pseq == 1:
            minlat = p.latitude
            maxlat = p.latitude
            minlon = p.longitude
            maxlon = p.longitude
        else:
            minlat =min(minlat, p.latitude)
            maxlat =max(maxlat, p.latitude)
            minlon =min(minlon, p.longitude)
            maxlon =max(maxlon, p.longitude)

    o = type('ANONYMOUS', (object,), dict())
    o.minlat = minlat
    o.maxlat = maxlat
    o.minlon = minlon
    o.maxlon = maxlon
    o.points = points
    return o

def __ecef2lla(X, Y, Z):
    #WGS84 Parameters
    a = 6378137
    a2 = a * a
    f = 1 / 298.257223563
    b = a * (1 - f)
    b2 = b * b
    e2 = (a2 - b2) / a2
    ep2 = (a2 - b2) / b2

    # ECEF to LLA
    p = math.sqrt(X * X + Y * Y)
    o = math.atan2(Z * a, p * b)

    longitude = math.atan2(Y, X)
    latitude = math.atan2(Z + ep2 * b * math.pow(math.sin(o), 3), p - e2 * a * math.pow(math.cos(o), 3))
    N = a / math.sqrt(1 - e2 * math.pow(math.sin(latitude), 2))
    height = p / math.cos(latitude) - N
    return (latitude * 180 / math.pi, longitude * 180 / math.pi, height)   # See (1) for 180/pi

def __timestamp2iso8601str(t):    # See (1)
    t = datetime.datetime.utcfromtimestamp(t)
    return '%02d-%02d-%02dT%02d:%02d:%02dZ' % (t.year, t.month, t.day, t.hour, t.minute, t.second)

def toGPX(data):
    gpx = ''
    gpx = gpx + '<?xml version="1.0" encoding="UTF-8"?>\x0a'
    gpx = gpx + '<gpx\x0a'
    gpx = gpx + '    version="1.0"\x0a'
    gpx = gpx + '    creator="Skytraq Monitor - rcarrasco@talentus.cl"\x0a'
    gpx = gpx + '    xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"\x0a'
    gpx = gpx + '    xmlns="http://www.topografix.com/GPX/1/0"\x0a'
    gpx = gpx + '    xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd">\x0a'
    gpx = gpx + '<time>%s</time>\x0a' % __timestamp2iso8601str(time.time())
    gpx = gpx + '<bounds minlat="%s" minlon="%s" maxlat="%s" maxlon="%s"/>\x0a' % (data.minlat, data.minlon, data.maxlat, data.maxlon)

    gpx_seg = ''
    gpx_wpt = ''
    for p in data.points:
        if p.is_upoi:
            gpx_wpt = gpx_wpt + '<wpt lat="%s" lon="%s">\x0a' % (p.latitude, p.longitude)
            gpx_wpt = gpx_wpt + '    <ele>%s</ele>\x0a' % (p.height)
            gpx_wpt = gpx_wpt + '    <time>%s</time>\x0a' % (p.time)
            gpx_wpt = gpx_wpt + '    <name>%s</name>\x0a' % (p.name)
            gpx_wpt = gpx_wpt + '    <cmt>%s</cmt>\x0a' % (p.name)
            gpx_wpt = gpx_wpt + '    <desc>%s</desc>\x0a' % (p.name)
            gpx_wpt = gpx_wpt + '</wpt>\x0a'
            continue
        if p.is_full:
            if gpx_seg != '':
                gpx_seg = gpx_seg + '    </trkseg>\x0a'
            gpx_seg = gpx_seg + '    <trkseg>\x0a'
        gpx_seg =gpx_seg + '        <trkpt lat="%s" lon="%s">\x0a' % (p.latitude, p.longitude)
        gpx_seg =gpx_seg + '            <ele>%s</ele>\x0a' % (p.height)
        gpx_seg =gpx_seg + '            <time>%s</time>\x0a' % (p.time)
        gpx_seg =gpx_seg + '            <speed>%s</speed>\x0a' % (p.speed)
        gpx_seg =gpx_seg + '            <name>%s</name>\x0a' % (p.name)
        gpx_seg =gpx_seg + '        </trkpt>\x0a'
    if gpx_seg != '':
        gpx_seg = gpx_seg + '    </trkseg>\x0a'

    gpx =gpx + gpx_wpt
    gpx =gpx + '<trk>\x0a'
    gpx =gpx + '    <name>SkyTraq Monitor TrackLog</name>\x0a'
    gpx =gpx + '    <desc>SkyTraq Monitor GPS Data</desc>\x0a'
    gpx =gpx + gpx_seg
    gpx =gpx + '</trk>\x0a'
    gpx =gpx + '</gpx>\x0a'
    return gpx
